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Abstract. Accurate camera motion estimation is a fundamental build¬ 
ing block for many Computer Vision algorithms. For improved robust¬ 
ness, temporal consistency of translational and rotational camera ve¬ 
locity is often assumed by propagating motion information forward us¬ 
ing stochastic filters. Classical stochastic filters, however, use linear ap¬ 
proximations for the non-linear observer model and for the non-linear 
structure of the underlying Lie Group SE3 and have to approximate 
the unknown posteriori distribution. In this paper we employ a non¬ 
linear measurement model for the camera motion estimation problem 
that incorporates multiple observation equations. We solve the underly¬ 
ing filtering problem using a novel Minimum Energy Filter on SE3 and 
give explicit expressions for the optimal state variables. Experiments on 
the challenging KITTI benchmark show that, although a simple motion 
model is only employed, our approach improves rotational velocity esti¬ 
mation and otherwise is on par with the state-of-the-art. 

Keywords: Minimum Energy Filter, Lie Groups, Optimal Control, Vi¬ 
sual Odometry 


1 Introduction 

Camera motion estimation is an important task in autonomous driving for 
which the ego-motion of the camera is fully determined by images from cameras 
mounted on the car. Most approaches require only temporal correpondences [3] 
or additional depth information ECU e.g. obtained from stereo estimation. Given 
two frames and a depth map, the underlying motion of the camera can be de¬ 
termined uniquely. However, two-frame methods are sensitive to noise and thus 
past information needs to be propagated with filtering approaches. Stochastic 
filters require assumptions about the a posteriori distribution, which is often 
unknown and thus can not be modeled adequately. Furthermore, an adaption 
to Lie groups is unknown for almost all stochastic filters. Application of state- 
of-the-art particle filters is limited due to the high amount of required particles. 
Mortensen m derived a second order deterministic Liter for the classical fil¬ 
tering problem on R” based on classical control theory. This result has been 
generalized to Lie groups m- 
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In this article we present a filtering model with state and observation equa¬ 
tions for the camera motion estimation problem. We adapt the approach m 
to this model on SE 3 and generalize it to incorporate multiple measurement 
equations depending non-linearly on the camera motion. We also show how the 
abstract exponential functor HU can be computed explicitly and finally derive a 
matrix representation of the inverse Hessian operator on SC 3 using special Kro- 
necker products. Numerical experiments show the fast convergence of the filter. 

Related work. The task of estimating the current state of a dynamical system 
only based on past observations of the system is known as filtering. In the last 
century numerous stochastic filters have been developed starting from the semi¬ 
nal work of Kalman m- See |3| for an overview and background. Since for non¬ 
linear dynamical systems with non-Gaussian noise processes this problem cannot 
be solved exactly, several approaches tried to cope with these non-linearities [5]. 
In general, as the a-posteriori distribution is unknown, most Gaussian filters 
are doomed to fail. State-of-the-art particle filters [7J alleviate the problem of 
not knowing the distribution. For large dimensions, however, generating enough 
particles becomes infeasible. Brigo et al. [5] use exponential families to model 
the a posteriori distribution, but the choice of an admissible sufficient statistic 
is critical and too restricted for our multiple measurement model. 

For the considered filtering problems on Lie groups we have to take into ac¬ 
count the non-linear geometry of the manifold to find an optimal filter. Markley m 
worked out a method for filtering problems on SO 3 whereas [M] investigates par¬ 
ticle filters on SE 3 . While the dimension of the embedding space is not excessively 
large (e.g, 16 for SE 3 ), the generation of samples on the respective Lie group 
is considerably more expensive. Mortensen m derived a deterministic and re¬ 
cursive second order optimal filter based on results of control theory and the 
dynamic principle. In the last years this approach was generalized to specific Lie 
groups [20117) . In various scenarios it has been shown that minimum energy fil¬ 
ters have an exponential convergence rate m and perform superior to extended 
Kalman filters [50] ■ 

Contributions. 

— Formulation of filtering equations for the camera motion estimation problem 
on rigid scenes with constant motion assumption; 

— adaptation of the second order Minimum Energy Filter P23 such that it 
incorporates multiple and non-linear measurement equations; 

— derivation of explicit ordinary differential equations of the optimal state and 
the inverse Hessian for which we derive a matrix representation; 

— experiments that show the comparable performance in accuracy of the cam¬ 
era motion against a state-of the art-method [ 8 ] on the challenging real-life 
KITTI benchmark. 

Preliminaries. We use the notation SE 3 to denote the special Euclidean group 
equipped with its tangent space Te SE 3 and Lie algebra SC3. Tangent vectors 
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ET £ Te SE 3 are obtained as evaluations of left-invariant vector fields T)L E (/) [-T] 
that one-to-one correspond to the tangent vector r £ se 3 . Here, LeF = EF 
denotes left-translation and D Le denotes the differential of L E - SE 3 can be 
identified with a matrix Lie subgroup of GL(4). We adopt the Riemannian 
metric (X,Y) E := (E~ 1 X, E~ 1 Y),WX, Y £ T e SE 3 , where {A, B) = tr (A T B) 
denotes the canonical matrix inner product. The Riemannian gradient grad / 
of a differentiable function / : SE 3 —>■ R is defined through the directional 
derivative Df(E)[Ef2] =: (grad/(E), Efl) E for all Q £ sc 3 . The Riemannian 
Hessian Hess f(E) [•] : T E SE 3 —> T E SE 3 at E £ SE 3 is defined through the 
relation (Hess f(g)[Er], Ef2) := D(D f{E)[Efl})[Er] - T>f(E)[V Er Ef2\ for all 
r, El £ se 3 . Here, V denotes the Riemannian (Levi-Civita) connection. The Lie 
algebra se 3 can be associated with a 6 -dimensional vector space and we define 
the operation vec sc : se 3 —>■ R 6 given by 
0 -73 72 74 \ \ 

-72 7°1 "o 1 76 ) ) = ( 7 l, 72 , 73 , 74 , 75 , 76 ) T . ( 1 ) 

0 0 0 0 '' 

We denote the inverse operation by mat sc : R 6 —» se 3 and the projection onto 
the Lie algebra by Pr : GL(4) —> SC 3 . The standard basis of se 3 is given by 
{Bi = mat sc ( 6 ,;)}, where 6j, i = 1,..., 6 is the standard basis of R 6 . 

2 Minimum Energy Filtering Approach 

The classical filtering problem consists of a state equation that describes the 
dynamics of an unknown state E(t) and observation equations connecting mea¬ 
surements to the state of the system. These real-valued equations are given by 

E(t)=f t (E(t)) + 5(t), E(0)=E 0 , (state) (2) 

y(t) = h t (E(t)) + e(t), (observation) (3) 

where the functions ft and h t describe the state and observation dynamics, 
respectively, and 5(t),e(t) are noise processes. Stochastic filters usually under¬ 
stand these equations as stochastic differential equations and try to find for 
each t the maximum of the a posteriori distribution P(E(t)\y(s), s < t). In con¬ 
trast, Mortensen m investigated © and m from control theory point of view: 
He considered the equation 0 as a dynamical system, controlled by a control 
process S(t) such that the residual ||e(f)|| = || y(t) — h t (E(t))\\ is minimized. 

2.1 Measurement Model for Ego-Motions 

Supported Models. In this section we derive an optical flow observer model 
with corresponding state equation on the Lie group SE 3 . They support two 
models that incorporate two different kinds of given data, i.e. 

— temporal optical flow and stereo matches (stereo approach), 

— temporal optical flow and depth map (monocular approach). 

Since the proposed filter supports both models and shares the same derivation, 
we only consider the monocular model in the following. 
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Optical Flow Induced by Egomotion. In this work we denote the time space 
by T := R>o and the image sequence recorded by a camera moving through a 
static scene by / = {ft,t eT}. Let (R(t), v(t)) = E(t) G SE 3 be the incremental 
camera motion from frame f t to ft,+i • W.l.o.g. at time t we set the coordinate 
system to be identical to the one of the camera recording f t , i.e. the extrinsic 
camera parameters are C t = {1,0) and C t + 1 = (R(t),v(t)). Let I € M 3 be 
a scene point and we denote its perspective projection into camera Ct by x l = 
(x\, x\, 1) T = Pc{X) where Pc{(x 1 , £ 2 , £ 3 ) T ) := x^ l {x\, X 2 , £ 3 ) T . Furthermore, 
we denote by d{x t ) G R the depth of X in camera C t such that the scene point 
can be reconstructed by X = x t d{x t ), see Fig. [I] 



Fig. 1 . Setup for temporal optical flow for either a given depth map or stereo matchings. 
Correspondences are given by x t+1 = x* + u{x t ’ 1 ), and x t+1 = v(t) + R{t)x t+1 denotes 
the perspective projection of X to the camera plane of Ct+ 1. 


With this relation we obtain the optical flow induced by the camera mo¬ 
tion (R(t),v(t)) and depth d{x t ): 

u{R{t),v{t),x t ,d{x t )) + x* =Pc(^R{t) T {x t d{x t ) — v{t))^. (4) 

We introduce index k to distinguish multiple observations and reformulate the 
observer equations in terms of E(t) = {R{t),v(t)) G SE 3 yielding 

u(E(t),g k (t)) + xl = p c {iE(t)- 1 gk (t)), f: =( 8 J? 8 ) (5) 

where gk{t) = g{t\ x\, d{x t k )) := {d{x t k ){x t k ) T , 1) T G R 4 for a single measurement. 

3 Minimum Energy Filter Derivation 

State and observation equations. As we want to recover the camera motion 
from the image data, we cannot incorporate any prior knowledge of the cameras’ 
kinematics such as data from external acceleration sensors. The only assumption 
we make is to demand a constant camera motion E(t) G SE 3 that is influenced 
by a noise process 6(t) G sc 3 , which also models accelerations. This kinematic 
state equation on SE 3 without dynamics ft (i.e. ft = 0) is given by 

E(t) = E(t)5(t) G T m SE 3 , E{ 0) = E 0 G SE 3 . 


( 6 ) 
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We incorporate multiple flow observations y k {t) '■= u k {E{t), g k {t),t) + Xk at 
different image points Xk for k G {1,..., n} that depend on the ego-motion E(t) 
and are corrupted by noise vectors ek(t) G R 3 . This gives 

Dk(t) = h k {E{t)) + e k (t ), k G {1,... ,n} . (7) 

Here we used the observation functions h k : SE 3 xE 4 —>• R 3 , k G {1 ,,n} that 
we define as is Eq. ©, i.e. h k {E{t)) = h k (E(t),g k (t)) := P c {IE{t)~ l g k {t)). 

Energy function. We want to find the camera motion that describes the ob¬ 
servation process best up to a small error e, i.e. we want to minimize the resid¬ 
ual \\e k (t)\\ 2 Q (INI 2 q ■= ' T Q-) for all t such that the dynamical system on E(t) 
© is also fulfilled. The latter means that also the error term ||vec sc (d(f))||| is 
minimized. Here S G R 6x6 and Q G R 3x3 are symmetric and positive definite 
weighting matrices. We define the following energy function 

J(e, S,t 0 ,t) := m 0 (E 0 ,t,to) + / c(S, e,r,t) dr, ( 8 ) 

Jt 0 

where e := {e/,-, k = 1,..., n}, S := (<5(r), r <t),t GT, c : se 3 x R 3n xT xT —»• R 
is a quadratic penalty function for 6 and e given by 

n 

c(S,e,r,t) := |e““ ( *“ r) (||vec 5 c ((5(T))|||+^]||e fc (r)||^ (9) 

k =1 

and mo : SE 3 xT x T —> R>o, (Eo,to,ti) i->- \e~ a{ ' t ~ t °' > tr((Eo — 1 ) T (£’ 0 — 1)), 
with 1 being the identity matrix in R 4x4 , is a penalty function for the initial 
condition. Here we also used the idea of a decay rate a > 0 from m at which 
old information is forgotten. To incorporate the observations 0 we substitute 
the error term e k (t) = e k (E(t),t) by y k (t) — h k {E{t)) in Eq. ©. 

Optimal control problem. The optimal control theory allows us to determine 
the optimal control input <5 that minimize the energy J ( e(E(t ), t), 5 , to, t) for each 
t G T subject to the state constraint ©. To be precise, we want to find <5|[t 0 ,t] 
for all ( G T and fixed E G SE 3 defining the value function 

V(E(t),t) := min J{e(E(t), t),5, t 0 , t) s.t. E(t) = E{t)6{t), E{ 0) = E 0 . (10) 

%o,*l 

The optimal trajectory is E*(t) := argmin B ( t ^ eSE3 V(E(t),t) for all t G T and 
V(E, to) = rno(Eo,to, to)- This problem is a classical optimal control problem, for 
which the classical Hamilton-Jacobi theory jlOI2j gives the well known Hamilton- 
Jacobi-Bellman equation. Pontryagin [2] proved that the minimization of the 
Hamiltonian gives a solution of the corresponding optimal control problem {Pon¬ 
tryagin’s Minimum Principle). 

However, since SE 3 is a non-compact Riemannian manifold we cannot apply 
the classical Hamilton-Jacobi theory for real-valued problems (cf. [2J). Instead 
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we follow the approach of Saccon et al. m which derive a left-trivialized, optimal 
Hamiltonian based on control theory on Lie groups uni, which is given by H. : 
SE 3 xse 3 x se 3 x T —> R, 

H~(E,ij,,5,t) = c(5,e(E,t),to,t) - (v,5) ■ (H) 

The minimization of (HD w.r.t. the variable 6 leads HD Prop. 4.2] to the optimal 
Hamiltonian 'H~(E, p,,t) := / H~(E, /a, 6*, t), where vec se (S*) = e «(*- t o) 5'- 1 vec st (/j,). 
is given by 

n 

H~{E,n,t ) = ^||y fc -/i fe (L;)||Q-ie a(t_ * o) (^,mat 5 c (S ,_1 vec se (M))> • 

k =1 

( 12 ) 

In the next section we compute explicit ordinary differential equations for the 
optimal state E*(t) for each t € T that consists of different derivatives of the 
left trivialized Hamilton function (fl2l) . 


3.1 Recursive Filtering Principle of Mortensen 

In order to find a recursive filter we compute the total time derivative of the 
optimality condition on the value function, which is 

grad-L V(E*,t) = 0, (13) 


for each t G T. This equation must be fulfilled by an optimal solution of the 
filtering problem E* G SE 3 . Unfortunately, because the filtering problem is in 
general infinite dimensional, this leads to an expression containing derivatives 
of every order. In practice (cf. [20117] 1 derivatives of third order and higher are 
neglected, since they are complicated to compute. Omitting these leads to a 
second order approximation of the optimal filter. 


Theorem 1. The differential equations of the second order Minimum Energy 
Filter for our state (0 and nonlinear observer 0 model is given by 


E* =^E* m at sc (Pvec sc (^Pr(n fe (E;*)))), E*(t 0 )=E 0 , (14) 

k 

p = ^ aP + S -1 _ P^(f VBC „ (Pr(Xj>(B . ))) + D k (E*))P 


-r 


vec S ,(E* 


W + P (Cc^*-W T ’ P ( t 0 ) = P 0 , 


(15) 


where A k (E) = A k (E,g k ) := (re*, 1 / - n k 2 IE 1 e 3 gJl) T Q(y k - h k (E))gJE T , 
n k '■= n k {E) := eJlE~ 1 g k and Pr(H) := argmin fig 6 C 3 (l7, A). D k {E) is derived 
in the appendix in Eq. (1251) and the matrix valued functions P, t* : R 6 —>■ R 6x6 
come from the vectorization of the connection functions. Their components are 
given by {r z )ij '■= J2k=l P jk zk an d ( P z)ik '■= Y?j=\P l jk zi w ^ G R 6 and 
Christoff el-Symbols P l - k := Ff- from JfHj . 

The initial Pq G R 6x6 is given by P 3 vec 6c (I?) = vec 6c ((UHessmo(-E’o)[-E’12]) _1 ), 
and Eq is an initialization in SE 3 . 
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The sketch of the proof is given at the end of this section. For the proof we need 
some lemmas listed below, the proofs of which can be found in the appendix. 

We adapt the minimum energy filter for general Lie groups derived in m to 
our nonlinear measurement model on SE 3 . Following [171 Eq. ( 37 )] the estimate 
of the optimal state E* is given by 

E* = -E*Z(E*,tr 1 (g^d l n~(E*, f i,t))), ( 16 ) 

which contains the second order information matrix Z(E*R) : se 3 —> se 3 of the 
value function V (cf. (fTTfll h defined by Z(E*,t)(f2) >->■ E*^ 1 Hessi V(E*,t)[E* f2]. 
The gradient of the Hamiltonian in (1161) is given in the following lemma. 

Lemma 1. The Riemannian gradient on Te SE 3 of the Hamiltonian Ti~ (E, /x, t) 
with Ak{E) = Ak(E,gk ) defined in Theorem.^ is given by 

gr & d 1 H-{E^R)=e- a{ - t - t ^Y, E ^<ME)). ( 17 ) 

fc 

In order to derive a second order filter, we also need a recursive expression of 
the operator Z, that has been derived in mi Eq. (51)] and is approximately 

J t Z (E*(t), t ) « cu* o Z(E*,t) + Z(E*,t) o 

+ Z(E*,t) o Hess 2 H~{E*, 0, t) o Z{E*,t) + FT -1 Hessi H~(E*, 0, t)oE*, 

where third order derivatives are neglected. For the computation of the Hes¬ 
sian we need implicitly the Riemannian connection V with connection function 
ujqA := F7V a A. The dual operator uj* q - is given by (u* n A,0) := (A,ujoO). 

Next, we derive a matrix representation for all terms in Eq. m provided by 
the vec sc —operation defined in section [T] and the following lemmas. 

Lemma 2 (Matrix representation of Z). Let Z{E*,f) : SC 3 — > SC 3 be the op¬ 
erator in equation m- Then there exists a matrix K = K(t ) 6 R 6x6 such that 
we can vectorize Z(E*,t)(f2) for each 12 £ SC 3 . Then it holds vec se (Z(E*,t)(f2)) = 
K(t) vec st (f2), and thus vec se (d/dtZ(E*,t)(f2)) = K{t) vec sc (12), as well as 

1 . vec 6c (w^,_ l£ ;,Z(F;*,t)ol2) = (r v * eCae(E ,_ 1 ^ ) ) T R:(t)vec 6c (l7) 

vec se (Z{E*,t) °u E ,- lE ,f2) =i^(t)r^ Cj(( _ E ._ 1 _g. ) vec se (f2) , 

3. yec sc (Z{E*,t){Hess 2 H~(E*,0,t)[Z(E* S ,t){H)})) 

= - e Q ( t -*°)/v(t)5'- 1 Jv(t)vec«(12), 

with E and T* from Thm. Q] 

Finally we have to apply the vec 6c —operation to the last remaining term in (1181) : 


Lemma 3. It holds 

vec sc (FT -1 Hessi H~{E*,0,t)[E* 12]) 
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Fig. 2. Comparison of the rotational error (top, in degree) and the translational (bot¬ 
tom, in meters) of our approach and Geiger et al. [8] on the first 250 frames of se¬ 
quence 0 of the Kitti odometry benchmark. We used the parameters (a = 2, S = 
diag((si, si, si, S2, S2, S2)), si = 10~ 3 , S2 = ICC 6 , Q = 0.021). The dotted lines indicate 
the mean errors. In the translational part (bottom) both methods are competitive. In 
the rotational part (top) we outperform [8]. 


= ^(r v ec 6 e (Prh 4 fc (£*))) + D k (E*)) vec 5 c (I2) 

k 

where D k (-) : SE 3 — > ffi 6 * 6 and E : R 6 —> ffi 6 * 6 are given in the appendix. 

Sketch of proof of Thm.. [7J Insertion of (flTl) into (ITcJIl and vec sc (Z(E*, f)(l7)) = 
K(t) vec sc (I2) (cf. Lem. [2]) give 

E* = -e-^ t ~ t ^E*m a t 5c ( K K- 1 vec, c {J2E*PiiA k (E*)))y (19) 

k 

By evaluation of (fI51) at 17 G SC 3 and application of the vec sc —operation to both 
sides of m we obtain with Lemmas [2] and [3] the following dynamics of K : 

Kvec s Jf2) = ((f* + - e ait ~ to) KS^K 

vec se {E* l E*) J vec se (E* L E*) 

+ ( e -«(t-*o)^(/; eCis( p r(Afe( ^ ))) +£,(£*)))) vec sc (l 7 ). ( 20 ) 

k 

Since 17 was chosen arbitrarily we can neglect vec sc (I7) on both sides of (l20l) . 
A change of variables P(t) := (f ) -1 in dD and ( 1201 ) gives the ODEs 

m and m in Theorem IT) For brevity we omit the computations here. □ 

3.2 Numerical Geometric Integration 

In order to solve the differential equations (fTP) and (fT51) we use Crouch-Grossman 
methods [S]. We adapt the version for right invariant Lie groups from the stan¬ 
dard literature by permuting the order of the factors, i.e. 


E n+1 = E n Exp(|Af )Exp(|Af) 


(21) 
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sequence 

0 

1 

2 

3 

4 

5 

Geiger [S] trans. err. (m) 

0.023 

0.050 

0.027 

0.017 

0.017 

0.017 

ours trans. err. (m) 

0.027 

0.84 

0.060 

0.020 

0.024 

0.027 

Geiger 0 rot. err. (deg) 

0.27 

0.15 

0.26 

0.25 

0.10 

0.22 

ours rot. err. (deg) 

0.19 

0.17 

0.28 

0.22 

0.10 

0.22 


Table 1. Comparison between our approach and Geiger et al. [8] on the first six 
sequences of the KITTI visual odometry benchmark. Our approach is usually better 
than [8] in the rotational part, since we model the Lie Group explicitly, but inferior in 
the translation since our input data from [18] is often not correct, (cf. seq.1,2). 


with Ki := (j)(E n ) and K .f := (j)(E n Exp(ZrA'f')) and step size h. We use the 
method (I2T1) to integrate equation (fT4l) . where E*^ 1 (j>{E*) is defined by the right 
hand side of (HI. For the integration of equation (fl5l) we used a standard 2-stage 
Runge-Kutta schemes on M 6 * 6 , since P does not lie on a non-trivial manifold. 

4 Experiments 

Preprocessing. We computed the depth map from stereo images with 49] and 
the temporal optical flow between left images by the method 18] ■ Both methods 
are the top ranked on the Kitti Benchmark and the code is publicly available. 
To remove outliers in the flow / depth map we computed for each image on 50 
points x the energy E(x) := \\y{x) — h(x,E )||, and removed all points x with 
E{x) < A, where we selected A as 80% quantile the energy of all points. 


Evaluation on KITTI benchmark. We compare our approach with Geiger et 
al. (Si on the challenging KITTI benchmark. We evaluated the first six sequences 
of the KITTI benchmark. Both algorithms have as initialization the identity ma¬ 
trix, i.e. Eq = 1 thus it takes some frames until the approaches converge. For 
this reason, we omitted the first 10 frames in the evaluations. The translational 
and rotational error of our approach and Geiger et al. 0 w.r.t. ground truth ego- 
rnotion in depicted in Tab. [I] Usually, in the rotational component our approach 
works better than [8], since we model rotations explicitly on a manifold, as de¬ 
picted in FigH] However, our approach less exact in the translational component 
because the optical flow estimation by [IS] fails on some frames of sequences 1 
and 2 yielding a high energy and error. 


Synthetic Data. We evaluated our method on synthetic data with known depth 
maps and optical flow. Fig. [3] (top) shows the convergence from a wrong initializa¬ 
tion for different weights of the penalty term of 5, i.e. S t = A; diag(si, Si, Si, S 2 > $ 2 ) S 2 
with Si = 1CT 3 , S 2 = 10 -6 and A i = 10* for i = 0,..., 4, a = 0 and Q = 0.11. In 
frames 21 and 51 constant motion assumption is violated, leading to a high error. 
However, for small weights of the penalty term of 6 the filter converges almost 
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Fig. 3. Evaluation of our method on synthetic data. In the top row we observe the linear 
convergence in the logarithmic scale, which means exponential convergence behavior. In 
frames 21 and 31 there is an immediate change of direction which causes errors since the 
motion-constancy assumption is violated. However, our method adapts to the change 
in the subsequent steps. In the bottom row we consider multiplicative Gaussian noise 
with mean 1 and standard deviation a to the input data (optical flow). For little noise 
rates (er < 0.01) we obtain the accuracy required for practical applications (dotted 
lines): (0.1 degree, 0.05 meters). 


immediately. Fig. [3] (bottom) shows the performance of the filter on data dis¬ 
torted by multiplicative Gaussian noise. For high noise rates (a > 0.1) the filter 
fails while for small noise rates (a < 0.05) filter results have an accuracy compa¬ 
rable to state-of-the-art filters on real data. On the other hand this means that 
the input data is allowed to be wrong up to 1% in order to reach state-of-the-art 
results. For our evaluations on Tab. [T] this was not always the case. 

5 Conclusion 

We presented a second order Minimum Energy Filter with non-linear observation 
equations for the ego-motion estimation problem and derived explicit differential 
equations for the optimal state. Our experiments showed that our approach is 
comparable with the state-of-the-art approaches [5] : In the translational com¬ 
ponent our method is inferior but it is superior in the rotational component. 
The experiments also confirm the exponential convergence rate and robustness 
against multiplicative noise. 

In future work, we will generalize our model by allowing an acceleration of 
the camera. We expect this generalization to reduce the error in the translational 
part. Moreover, we plan to design a filter for monocular depth map estimation. 

Acknowledgments.This work was supported by the DFG, grant GRK 1653. 
The final publication is available at link.springer.com 

A Proofs of Lemmas [lH3] 

Proof (of Lem.W- We begin with the directional derivative of hk which is 

D h k (E)[E!2] = -n^IQE-'gk + K~ 2 {eJ I^E~ l g^IE^gk, (22) 
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where n k = Kk{E) := eJlE x g k . Then the following holds: 

n 

T> 1 'H~(E, n, t)[EQ] = -e-“( t -‘o)^t r (D/ lfc (£;)[£;r2](y fc -h k (E)) T Q) 

k= 1 
n 

= e-^-^ J2 <(«k ^ ~ % 2 /S- 1 e 3 ^/) T Q(y fc - h k (E))gJE~ T , 12) . (23) 

fe=1 

We obtain the Riemannian gradient on SE 3 by projecting (cf. [Q Sec. 3.6.1]) the 
left hand side of the Riemannian metric in (1231) onto Te SE 3 , which is 

gradi H~ (E, n, t ) ^ E Pr(A fc (E)). 

k 

with A k (E) := (k)) 1 / - n^ 2 IE~ 1 e 3 g k l) T Q(y k - h k {E))gJE~ T and Pr is this 
mentioned projection. □ 

Proof (of Lem. HP- The existence of a matrix I\ such that I\ vec sc (12) = Z(E *, t) [12] 
follows from considering the basis of sc 3 (cf. Sec. Q]) and also its inverse repre¬ 
sentation. The Hessian of the Hamiltonian w.r.t. the second argument evaluated 
at zero is vec 6 c (Hess 2 T~L~(E*, 0, i)[12]) = —vec sc (12), thus 

vec st (Z(E*, t)(Hess 2 H~{E*,0, t)[Z{E*,t)(L2)})) = -e^-^KftfS^Kft) vec se (12). 

For left-invariant Lie groups the vectorized representation of affine connection 
holds (cf. Q]), i.e. 


vec s t (tjJnA) = r veCst ( A ) vec «(^) + vec 6 c (DA[12]), (24) 

where matrix f veCee (A) has the components ( r veCse {A))ij = J 2 k=i^jk^ k - Note 
that we yield the definition in [I] by exchanging i and j. In the case of a constant 
function A we have DZ\[12] = 0. For the dual expression it holds for all 12, A G 
sc 3 : vec st (u* n A) = rf eCst{A) vec sc ( 12 ) with (r* eCse{A) )ik = Ej=i H k AP □ 

Proof (of Lem. Up. For any matrices A,B& R 4x4 and 12 G se 3 let < 8 >< i e) ( 8 ) Je 
denote the operators which extract the vector form of 12 and are defined as 
vec SC {AQB) =: (A B) vec sc (12) and vec 6 c (A12 T l?) =: (A <g>J c R) vec sc (12), 
respectively, for which explicit expressions exist. By definition of the Hessian, 
Lemma [1] and with Eq. (HH1) we obtain 

vec sc (2 ?* _1 Hessi H~(E*, 0, i)[£7*12]) = vec 5 C (£’*“ 1 V B ..R grad x H~(E*, 0, t )) 
=e -a(t-t 0 ) ^^f veCst ( Pr ( Afc(E ,))) vec sc (12) + vec se (D B . Pr (A k (E*))[f2})^j . 

k 

It can be shown that we can omit the projection Pr, i.e. vec sc (D Pr(Afc(.E))[12]) = 
vec sc (D(Hfe(£’))[12]) for all 12 G se 3 . After computing the directional derivative 
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of A k (E) in direction 17 we apply the vec 6c — operation and extract with the 

<S> S e — 5 ®sc— operations the direction 17. This gives 

vec sc (DAfc (E) [17]) = 2 I T Q(y k - h k (E))eJIE' 1 ® 3t E~ l g k g k E~ T 

- 2 K fi T e 3 eJiE~ 1 ® sc E~ x g k gjE~ T I T Q(y k - h k (E))gJE~ r 

+ K fi T e 3 gjE~ T ®J e E~ r I T Q(y k - h k (E))gJE~ r 

- - K k 2 i T e 3 gjE- r i T )Q(y k - h k (E))gJ E~ r ®J C £T t 

- (k^I t - K ^i T e 3 g^E- T i T )iE- 1 g k ejQIE- 1 ® 6C E~ 1 g k gJE~ T 

+ ( K k 2 I T - K k 3 I Te 3g k E~ T i T )QIE- 1 ® se E-'gkgjE-^ vec 5 C (l7) 

=:D k (E)ve c 5C (17). (25) 
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